#include "dash_simplex.h"
#include <vector>


void VCOPY(double *X,double *Y, int N);
void SSORT(double *C,int *IR, int N);
void SACONT2(int K, double **V, double *C, int N, int *IR, Function *SA_FCN);
void SAEXPAND(double& CL, double* X0, double* X00, double* XC, int N, double& C0, 
              double G, Function *SA_FCN);
void SACONTRACT(double CH, double CS, double C0, double& C00, double* X0, double *X00, 
                double *XC, double *XH, double B, int N, Function *SA_FCN);
void  SASIMP0(double** V, double* D, double* C, int* IR, int N, Function *SA_FCN);
void SASIMPLEX(double** V, int N, double* D, double** EX, double* C, int* IR, 
               int MX, Function *SA_FCN);
void XCENT(double** V, double* XC, int IH, int N);
void XZERO(double* XH, double* X0, double* XC, int N, double A);
void VRFILL(double* X, double A, int N);

void dashSimplex(double *X, double steplength, int N, double& TheChiSqd, Function *SA_FCN) //SA_SIMOPT(X,DX,N,TheChiSqd)
{
/*
! Purpose
!     A SIMPLEX-based routine which optimises the values of the N
! parameters pertaining to the components of the vector X
!
! Parameters
!   ARGUMENT  TYPE  I/O  DIMENSION  DESCRIPTION
!    N        I*4    I       -      No. of parameters to be optimised.
!    X        R*4    I       N      Initial guess.
!    X        R*4    O       N      The optimal answer.
!    DX       R*4    I       N      Initial step-lengths for X.
!
! Other Requirements
!     The user must provide a FUNCTION SA_FCN(N,X) which evalutes
! Chi-squared (un-normalised) given the vector X(N).
!
! History
!     D. S. Sivia    9 Feb 1995  Initial release.
!
! MVAR is the maximum number of variables allowed i.e. max d.o.f
*/


  //vector<double> DX;

  double* DX;
  newA(DX, N);

  for (int i = 1; i <= N; i++)
  {
    DX[i] = steplength;
  }

  const int MVAR = 1000;

  const int MAXITR = MVAR;

  // attempt to convert REAL V((MVAR+1)*MAXITR) which I believe from looking
  // at comments in other file is suppose to hold a V(N,*) 2D array, which translated
  // into C++ becomes V[*][N]..... However note..... that..... MAXITR=MVAR above.....
  double** V;
  newM(V,MAXITR,MVAR+1);

  // attempt to convert REAL EX(3*MVAR) which is believed to EX(N,*) in fortran and EX[*][N]
  // in C++......
  double ** EX;
  newM(EX,3,MVAR);

  double C[MVAR+1 + 1];  // added another 1
  int IR[MVAR+1 + 1]; // Maximum index I could find: N+1. Maximum value for N should be MVAR
                         // IR seems to hold a sorted list of pointers into C


  VCOPY(X, &V[1][0], N);



  double CHIMIN = SA_FCN->Value( &V[1][0]); //(N,V)

  double ZERO = 0.0;
  int ITER = 0;

  double CHI;

  bool done = false;

  while (done == false)
  {
    ITER++;
    if (ITER > MAXITR)
    {
      cout << "ITER > MAXITR in dashSimplex\n";
      exit(1);
    }

    SASIMPLEX(V,N,DX,EX,C,IR, N*1000, SA_FCN);
    VCOPY(&EX[1][0], &V[1][0], N);
    CHI = SA_FCN->Value(&V[1][0]);
    
//! Convergence criterion
    if (CHI > CHIMIN)
    {
      cout << "CALL DebugErrorMessage('CHI .GT. CHIMIN in SA_SIMOPT')\n\n";
      //exit(1);
    }

    if ((1.0-CHI/CHIMIN) > 0.00001)
    {
      CHIMIN = CHI;
    }
    else
    {
      done = true;
    }
  }

  VCOPY(&V[1][0], X, N);

  TheChiSqd = CHIMIN;  // changed from TheChiSqd = CHI;

  deleteA(DX);
  deleteM(V);
  deleteM(EX);
}


void VCOPY(double *in, double *out, int N)
{
      for (int I = 1; I <= N; I++)
        out[I] = in[I];
}


void SSORT(double *C,int *IR, int N)
{
      IR[1] = 1;
      for (int J = 2; J <= N + 1; J++)
      {
        for (int I = 1; I <= J - 1; I++)
        {
          if ( C[J] < C[IR[I]] ) continue; //GOTO 10
          for (int II = 1; II <= J - I; II++)
            IR[J+1-II] = IR[J-II];
          IR[I] = J;
          break; //GOTO 20
        }
  // 10   ENDDO
        IR[J] = J;
      }
   //20 ENDDO
}

/*
  N - number of parameter to be optimised
*/
void SACONT2(int K, double **V, double *C, int N, int *IR, Function *SA_FCN)
{
  // In fortran code: REAL V(N,*). 
  // In C++ code double ** V[*][N] assumed numerical recipies style

  for (int J = 1; J <= N + 1; J++)
  {
    if ( J == K )
    {
      SSORT(C,IR,N);
      return;
    }
    for (int I = 1; I <= N; I++)
    {
      V[J][I] = 0.5*(V[J][I]+V[K][I]);

      // Attempting to translate C(J) = SA_FCN(N,V(1,J))
      // V(1,J) passes the J'te column of matrix V
      // or more specifically N,V(1,J) means to pass elements V(1:N,J) to SA_FCN
      //
      // The line below should do the same trick. However, note the V array now is
      // assumed to take the form V(*,N). In C/C++ it is the second array index that
      // is run over fastest. Passing &V[J][0] means passing the pointer to J'te row

      C[J] = SA_FCN->Value(&V[J][0]);
    }
  }
}


void SACONTRACT(double CH, double CS, double C0, double& C00, double* X0, double *X00, 
                double *XC, double *XH, double B, int N, Function *SA_FCN)
{
  if (C0 < CH)
  {
    for (int I = 1; I <= N; I++)
    {
      X00[I] = B*(X0[I]-XC[I]) + XC[I];
    }
  }
  else
  {
    for (int I = 1; I <= N; I++)
    {
      X00[I] = B*(XH[I]-XC[I]) + XC[I];
    }
  }
  C00 = SA_FCN->Value(X00);
}


void SAEXPAND(double& CL, double* X0, double* X00, double* XC, int N, double& C0, 
              double G, Function *SA_FCN)
{
  for (int I = 1; I <= N; I++)
  {
     X00[I] = G*(X0[I]-XC[I]) + XC[I];
  }
  double C00 = SA_FCN->Value(X00);
  if (C00 < CL)
  {
    VCOPY(X00,X0,N);
    C0 = C00;
  }
}


void  SASIMP0(double** V, double* D, double* C, int* IR, int N, Function *SA_FCN)
{
  // In fortran code: REAL V(N,*). 
  // In C++ code double ** V[*][N] assumed numerical recipies style

  for (int I = 2; I <= N + 1; I++)
  { 
    // Attempting to translate VCOPY(V,V(1,I),N)
    VCOPY(&V[1][0],&V[I][0],N);  // see comment in SACONT2 function
    V[I][I-1] = V[I][I-1] + D[I-1];

    // Attempting to translate C(I) = SA_FCN(N,V(1,I))
    C[I] = SA_FCN->Value(&V[I][0]); // again see comment in SACONT2 function
  }
  SSORT(C,IR,N);
}


void SASIMPLEX(double** V, int N, double* D, double** EX, double* C, int* IR, 
               int MX, Function *SA_FCN)
{
  // In fortran code: REAL V(N,*), EX(N,*). 
  // In C++ code double ** V[*][N] and EX[*][N] assumed numerical recipies style

  bool SLOW;
  const double ALPHA=1.0, BETA=0.5, GAMA=2.0;

  SLOW = false;
  int N3 = 3*N;
  double CAIM = 0.0;  // in fortran code set to 0.0

  // attempting to translate C(1) = SA_FCN(N,V(1,1))
  C[1] = SA_FCN->Value(&V[1][0]);  // see comment in SACONT2 function

  SASIMP0(V,D,C,IR,N, SA_FCN);
  double CMIN = C[IR[N+1]];
  int ITER = 0;
  int NOLUCK = 0;
  int IRSTRT = 0;

  
  // 10 ITER = ITER + 1
  do
  {
    ITER = ITER + 1;
    NOLUCK = NOLUCK + 1;
    if (ITER >= MX || NOLUCK > 100)
    {
      SSORT(C,IR,N);
      // attempting to translate VCOPY(V(1,IR(N+1)),EX,N)
      VCOPY( &V[IR[N+1]][0], &EX[1][0], N);  // see comment in SACONT2 function

  //  } // not this is not the real end of if


      if (ITER < 250)
      {
        // see comment below concerning DSMALL for the reason to put in the
        // exit below
        cout << "This part of dashSimplex is hopefully never called....\n\n";
        exit (1);

        NOLUCK = 0;
        for (int ID = 1; ID <= N; ID++)
        {
  //            D(ID) = D(ID)/10.0
          D[ID] = D[ID]/2.0;
        }
        // attempting to translate VCOPY(EX,V,N)
        VCOPY(&EX[1][0], &V[1][0], N);  // see comment in SACONT2 function
        // attempting to translate C(1) = SA_FCN(N,V(1,1))
        C[1] = SA_FCN->Value(&V[1][0]);

        // Note Shankland suggested substituting 2nd arg in SASIMP0(V,DSMALL,C,IR,N) with D
        // Since DSMALL is not defined anywhere and hence if this function was actually called
        // would be a potential disaster
        CMIN = C[IR[N+1]];
      }
      else
      {
        return;
      }
    }
        
    
    // converting VRFILL(EX,0.,N3) to C++
    VRFILL(&EX[1][0], 0.0, N3);

      
    // converting CALL XCENT(V,EX(1,3),IR(1),N)
    XCENT(V, &EX[3][0], IR[1], N);

    // converting XZERO(V(1,IR(1)),EX,EX(1,3),N,ALPHA)
    XZERO( &V[IR[1]][0], &EX[1][0], &EX[3][0], N, ALPHA);
    
    double C0 = SA_FCN->Value(&EX[1][0]);
    
    double CL = C[IR[N+1]];
    double CH = C[IR[1]];
    double CS = C[IR[2]];
    
    if (C0 < CL)
    {
      SAEXPAND(CL, &EX[1][0], &EX[2][0], &EX[3][0], N, C0, GAMA, SA_FCN);
    }
    else if (C0 > CS)
    {
      double C00;
      SACONTRACT(CH, CS, C0, C00, &EX[1][0], &EX[2][0], &EX[3][0], &V[IR[1]][0], BETA, N, SA_FCN);
    
    
      if (C00 < CH && C00 < C0)
      {
        VCOPY( &EX[2][0], &EX[1][0], N);
        C0 = C00;
      }
      else
      {
        SACONT2(IR[N+1], V, C, N, IR, SA_FCN);
        IRSTRT = IRSTRT + 1;

        if ( C[IR[N+1]] < CMIN) CMIN = C[IR[N+1]];
        if ( IRSTRT >= 5)
        {
          NOLUCK = 0;
          SSORT(C,IR,N);
          VCOPY( &V[IR[N+1]][0], &EX[1][0], N);
          return;
        }
      }
    }
    
    VCOPY( &EX[1][0], &V[IR[1]][0], N);
    C[IR[1]] = C0;
    SSORT(C,IR,N);
    if (C[IR[N+1]] < CMIN)
    {
      double DROP = (CMIN-C[IR[N+1]])/CMIN;
      if (fabs(DROP) < 1.0E-4) SLOW = true;
      NOLUCK = 0;
      CMIN = C[IR[N+1]];
    }
  }
  while (CMIN > CAIM && !SLOW);

  VCOPY( &V[IR[N+1]][0], &EX[1][0], N);
}


void XCENT(double** V, double* XC, int IH, int N)
{
  // In fortran code REAL V(N,*). In C++ code assumed V[*][N]

  double XNORM = 1.0 / static_cast<double>(N);
  for (int J = 1; J <= N + 1; J++)
  {
    if (J == IH) break;
    for (int I = 1; I <= N; I++)
      XC[I] = XC[I] + V[J][I];
  }    

  for (int I = 1; I <= N; I++)
    XC[I] = XC[I]*XNORM;
      
}


void XZERO(double* XH, double* X0, double* XC, int N, double A)
{
  for (int I = 1; I <= N; I++)
    X0[I] = A*(XC[I]-XH[I]) + XC[I];
}


void VRFILL(double* X, double A, int N)
{
  for (int I = 1; I <= N; I++)
    X[I] = A;
}